#pragma once

#include <serial_driver/serial_port.hpp>
#include <io_context/io_context.hpp>
#include <sys/types.h>
#include <cstdint>
#include <geometry_msgs/msg/detail/twist__struct.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <mutex>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/detail/imu__struct.hpp>
#include <sensor_msgs/msg/imu.hpp>

#define SEND_DATA_CHECK 1            // 标志位，发送端做校验
#define READ_DATA_CHECK 0            // 标志位，接收端做校验
#define FRAME_HEADER 0X7B            // 帧头，和下位机一致
#define FRAME_TAIL 0x7D              // 帧尾
#define RECEIVE_DATA_SIZE 24         // 下位机发送过来的数据长度
#define SEND_DATA_SIZE 11            // ROS 发送给下位机的数据长度
#define ACCEL_SCALE 1672.0f          // 加速度数据需要除以1672得到 m/s^2
#define GYROSCOPE_SCALE 3753.0f      // 角速度数据需要除以3753得到 rad/s

/**
 * @brief 串口参数结构体
 */
struct SUsartParameters
{
    std::string usartPortName_ = {"/dev/ttyACM0"};
    int         usartBaudRate_{115200};
    uint32_t    timeout_{1000};
};

/**
 * @brief 速度数据结构体
 */
struct SVelocityData
{
    float x_{0.f};  // X轴线速度 (m/s)
    float y_{0.f};  // Y轴线速度 (m/s)
    float w_{0.f};  // 角速度 (rad/s)
};

/**
 * @brief 里程计数据结构体 (速度)
 */
struct SOdometryData
{
    rclcpp::Time timestamp_  = rclcpp::Time(0, 0);
    float        vxData_{0.f};   // X轴线速度 (m/s)
    float        vyData_{0.f};   // Y轴线速度 (m/s)
    float        vzData_{0.f};   // 角速度 (rad/s)
};

/**
 * @brief IMU数据结构体
 */
struct SImuData
{
    rclcpp::Time timestamp_ = rclcpp::Time(0, 0);
    double       linearAccelerationXData_{0.};  // X轴加速度 (m/s^2)
    double       linearAccelerationYData_{0.};  // Y轴加速度 (m/s^2)
    double       linearAccelerationZData_{0.};  // Z轴加速度 (m/s^2)
    double       angularVelocityXData_{0.};     // X轴角速度 (rad/s)
    double       angularVelocityYData_{0.};     // Y轴角速度 (rad/s)
    double       angularVelocityZData_{0.};     // Z轴角速度 (rad/s)
};

/**
 * @brief 机器人驱动主类，负责与 STM32 的串口通信
 */
class CTurnOnRobot
{
  public:
    CTurnOnRobot(const CTurnOnRobot&)                   = delete;
    CTurnOnRobot&        operator=(const CTurnOnRobot&) = delete;
    static CTurnOnRobot& getInstance();

    /**
     * @brief 打开串口
     * @param usartParameters 串口参数
     * @return 成功返回true
     */
    bool openUsart(const SUsartParameters& usartParameters);

    /**
     * @brief 关闭串口
     */
    void closeUsart();

    /**
     * @brief 检查串口是否打开
     */
    bool isUsartOpen();

    /**
     * @brief 启动接收线程
     */
    void startReceive();

    /**
     * @brief 获取机器人使能状态
     * @param enableRobot 使能状态(0=使能，其他=失能)
     */
    void getEnableRobot(uint8_t& enableRobot);

    /**
     * @brief 获取里程计数据(速度)
     */
    void getOdomData(SOdometryData& odomData);

    /**
     * @brief 获取IMU数据
     */
    void getImuData(SImuData& imuData);

    /**
     * @brief 获取电池电压
     */
    void getRobotVoltage(float& powerVoltage);

    /**
     * @brief 设置机器人速度
     */
    void setRobotVel(const SVelocityData& setRobotVel);

  private:
    CTurnOnRobot();
    ~CTurnOnRobot();

    bool          getSerialData();
    void          loopReceive();
    short         imuTrans(const uint8_t dataHigh, const uint8_t dataLow);
    float         odomTrans(const uint8_t dataHigh, const uint8_t dataLow);
    unsigned char checkSum(const unsigned char countNumber, const unsigned char mode);

  private:
    uint8_t        receivedSerialData_[RECEIVE_DATA_SIZE];
    uint8_t        sendSerialData_[SEND_DATA_SIZE];
    std::mutex     serialMutex_;
    std::shared_ptr<drivers::common::IoContext> io_context_;
    std::shared_ptr<drivers::serial_driver::SerialPort> mcuSerial_;
    uint8_t        enableRobot_{1};      // 默认失能
    SOdometryData  odomData_;
    SImuData       imuData_;
    float          powerVoltage_{0.0f};
    bool           isRunning_{false};
};
